﻿#include "CalibrateCamera.h"


CalibrateCamera::CalibrateCamera()
{
	_calibrating = false;

	_dictionary = aruco::getPredefinedDictionary(aruco::DICT_4X4_50);
	FileStorage fs("detector_params.yml", FileStorage::READ);
	bool readOk = _detectorParams.readDetectorParameters(fs.root());

	_board = new aruco::CharucoBoard(Size(squaresX, squaresY), squareLength, markerLength, _dictionary);
	_detector = new aruco::CharucoDetector(*_board, _charucoParams, _detectorParams);
}

void CalibrateCamera::set_calibrate_flag(bool flag)
{
	_calibrating = flag;
}

//复位
void CalibrateCamera::reset()
{
	allCharucoCorners.clear();
	allCharucoIds.clear();
	allImagePoints.clear();
	allObjectPoints.clear();
	allImages.clear();
}

//处理图像
void CalibrateCamera::process(cv::Mat mat)
{
	vector<int> markerIds;
	vector<vector<Point2f>> markerCorners;
	Mat currentCharucoCorners, currentCharucoIds;
	vector<Point3f> currentObjectPoints;
	vector<Point2f> currentImagePoints;

	//检测板
	_detector->detectBoard(mat, currentCharucoCorners, currentCharucoIds);

	//显示
	Mat img_copy;
	//mat.copyTo(img_copy);
	cvtColor(mat, img_copy, COLOR_GRAY2RGB);

	if (!markerIds.empty()) {
		aruco::drawDetectedMarkers(img_copy, markerCorners);
	}

	size_t t = currentCharucoCorners.total();
	if (t > 0) {
		aruco::drawDetectedCornersCharuco(img_copy, currentCharucoCorners, currentCharucoIds);
	}

	imshow("aruco out", img_copy);

	if (t <= 3) {
		qDebug() << QString("corners:%1 <= 3, return.").arg(t) << Qt::endl;
		return;
	}

	_board->matchImagePoints(currentCharucoCorners, currentCharucoIds, currentObjectPoints, currentImagePoints);

	if ((!currentImagePoints.empty()) && (!currentObjectPoints.empty())) {
		allCharucoCorners.push_back(currentCharucoCorners);
		allCharucoIds.push_back(currentCharucoIds);
		allImagePoints.push_back(currentImagePoints);
		allObjectPoints.push_back(currentObjectPoints);
		allImages.push_back(mat);

		imageSize = mat.size();
	}

	if (allCharucoCorners.size() >= 4) {

		int calibrationFlags = CALIB_ZERO_TANGENT_DIST | CALIB_FIX_ASPECT_RATIO;
		float aspectRatio = 1;

		Mat cameraMatrix, distCoeffs;
		if (calibrationFlags & CALIB_FIX_ASPECT_RATIO) {
			cameraMatrix = Mat::eye(3, 3, CV_64F);
			cameraMatrix.at<double>(0, 0) = aspectRatio;
		}

		double repError = calibrateCamera(allObjectPoints, allImagePoints, imageSize, cameraMatrix,
			distCoeffs, noArray(), noArray(), noArray(), noArray(), noArray(), calibrationFlags
		);
		bool saveOk = saveCameraParams(
			"camera_params.yml",
			imageSize, aspectRatio, calibrationFlags, cameraMatrix, distCoeffs, repError
		);

		if (saveOk) {
			QMessageBox::information(
				NULL,
				QString("校正参数"),
				QString("参数文件保存成功"),
				QMessageBox::Yes, QMessageBox::NoButton);
		}

		//显示标记的图片
		bool showChessboardCorners = false;
		if (showChessboardCorners) {
			for (size_t frame = 0; frame < allImages.size(); frame++) {
				Mat imageCopy = allImages[frame].clone();
				if (allCharucoCorners[frame].total() > 0) {
					aruco::drawDetectedCornersCharuco(
						imageCopy,
						allCharucoCorners[frame],
						allCharucoIds[frame]
					);
				}
				imshow("all aruco out", imageCopy);
			}
		}

	}

	//
}

//保存相机参数
bool CalibrateCamera::saveCameraParams(const std::string& filename, cv::Size imageSize, float aspectRatio, int flags,
	const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs, double totalAvgErr)
{
	cv::FileStorage fs(filename, cv::FileStorage::WRITE);
	if (!fs.isOpened())
		return false;

	time_t tt = 0;
	time(&tt);
	struct tm t2;
	localtime_s(&t2, &tt);
	char buf[1024];
	strftime(buf, sizeof(buf) - 1, "%c", &t2);

	fs << "calibration_time" << buf;
	fs << "image_width" << imageSize.width;
	fs << "image_height" << imageSize.height;

	if (flags & cv::CALIB_FIX_ASPECT_RATIO) fs << "aspectRatio" << aspectRatio;

	if (flags != 0) {
		sprintf_s(buf, "flags: %s%s%s%s",
			flags & cv::CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "",
			flags & cv::CALIB_FIX_ASPECT_RATIO ? "+fix_aspectRatio" : "",
			flags & cv::CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "",
			flags & cv::CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "");
	}
	fs << "flags" << flags;
	fs << "camera_matrix" << cameraMatrix;
	fs << "distortion_coefficients" << distCoeffs;
	fs << "avg_reprojection_error" << totalAvgErr;
	return true;
}


